from typing import Dict, List

from matplotlib import pyplot as plt

from agent import Agent
from util import *
from wall import Wall


class Simulation:
    time: int = 0  # 当前模拟的时间戳,不需要设置，默认从0开始
    time_gap: float = 0.01  # 模拟的时间步长，不需要设置，一定要与 Agent 保持一致

    maxLen: int
    agents_hashmap: Dict[int, Agent]
    walls: List[Wall]

    # distance_map: List[List[float]]  # 存储各个 agent 之间的距离，避免重复计算

    def __init__(self, agents: {}, walls: List[Wall]):
        Agent.walls = walls

        self.tolerance: float = 0.0
        self.agents_hashmap, self.walls = agents, walls
        self.maxLen = len(agents)
        self.distance_map = [[0] * (len(agents) + 1) for _ in range(len(agents) + 1)]
        self.time_gap = agents[1].time_gap

    def simulate(self):
        self.time += 1
        self._init_agents()
        self._find_neighbors()
        self._move_agents()
        self._draw()

    def _draw(self):
        self._draw_agents()
        self._draw_walls()

    def _draw_agents(self):
        plt.axis([0, 10, 0, 10])
        for _, agent in self.agents_hashmap.items():
            drawn_graphics = plt.Circle((agent.position[0], agent.position[1]),
                                        agent.radius)
            plt.gcf().gca().add_artist(drawn_graphics)

    def _draw_walls(self):
        for wall in self.walls:
            plt.plot([wall.start_point[0], wall.end_point[0]],
                     [wall.start_point[1], wall.end_point[1]],
                     color='r', alpha=1)

    def _find_neighbors(self):
        for i, agent in self.agents_hashmap.items():
            for j, other in self.agents_hashmap.items():
                if i != j and agent.can_reach(other.position):
                    agent.neighbors.append(other)

    def _move_agents(self):
        for _, agent in self.agents_hashmap.items():
            agent.time += 1
            self._update_v_for(agent)
            agent.move()

    def _init_agents(self):
        for name in list(self.agents_hashmap.keys()):
            agent = self.agents_hashmap[name]
            if agent.position[1] < 0:  # 删除离开的成员
                del self.agents_hashmap[name]
                continue

            if np.linalg.norm(agent.position - np.array([5.0, 0.0])) < 1:
                agent.toleration = 0.03
            else:
                agent.toleration = 0.01

            if agent.position[1] > 0.5:  # 更新目的地
                agent.destination = np.array([5.0, -0.4])
            elif 4.5 <= agent.position[0] <= 5.5:
                agent.destination = np.array([5.0, -0.4])
            elif agent.position[0] < 4.5:
                agent.destination = np.array([5.0, agent.position[1]])
            else:
                agent.destination = np.array([5.0, -agent.position[1]])

            v = agent.destination - agent.position
            agent.set_unit_v(v / np.linalg.norm(v))
            agent.neighbors = []

    @staticmethod
    def _update_v_for(agent: Agent):
        next_distance = agent.confirm_forward_distance(agent.v)
        if next_distance > agent.max_distance:  # 方向不变
            agent.set_speed(agent.max_speed)
            return

        next_v = agent.v
        for tmp_angle in change_angle_range(agent.max_deflection_angle,
                                            agent.unit_deflection_angle):
            tmp_v = agent.rotate(tmp_angle)
            tmp_distance = agent.confirm_forward_distance(tmp_v)

            if tmp_distance > next_distance:
                next_distance = tmp_distance
                next_v = tmp_v

        agent.set_unit_v(next_v).set_speed(next_distance / agent.time_gap)


if __name__ == '__main__':
    a1 = np.array([math.cos(0.5 * math.pi), math.sin(0.5 * math.pi)])
    a2 = np.array([math.cos(1.5 * math.pi), math.sin(1.5 * math.pi)])
    angle = cal_angle_between_a_b(a1, a2)
    print(angle, 1.5 * math.pi)
